/*
Tag ZBY
------------------------------------------
MPU6050/6500
pitch  roll  yaw accel[0]  gyro[0]
------------------------------------------

--------------------------------------------------------
OLED(HARDWARE SPI)
OLED_ShowString(0,7,(uint8_t *)"MPU6050 Demo",8);
--------------------------------------------------------
*/
#include "main.h"
#include "stdio.h"
#include "ti_msp_dl_config.h"

uint8_t oled_buffer[32];
char mode_str[20];
int mode = 3;
int situation = 1;
int sys_run = 0;
int delta;
float angel_init = 0;
int line_speed = 35;
int round_speed = 21;
int detect_speed = 10;

void motor_run(void) {
  sys_run = 1;
  DL_TimerA_startCounter(PWM_MOTOR_INST);
  DL_GPIO_clearPins(GPIO_MOTOR_CONTROL_PORT, GPIO_MOTOR_CONTROL_PIN_AIN1_PIN);
  DL_GPIO_setPins(GPIO_MOTOR_CONTROL_PORT, GPIO_MOTOR_CONTROL_PIN_AIN2_PIN);
  DL_GPIO_clearPins(GPIO_MOTOR_CONTROL_PORT, GPIO_MOTOR_CONTROL_PIN_BIN1_PIN);
  DL_GPIO_setPins(GPIO_MOTOR_CONTROL_PORT, GPIO_MOTOR_CONTROL_PIN_BIN2_PIN);
}
void motor_stop(void) {
  sys_run = 0;
  DL_TimerA_stopCounter(PWM_MOTOR_INST);
  DL_GPIO_setPins(GPIO_MOTOR_CONTROL_PORT, GPIO_MOTOR_CONTROL_PIN_AIN1_PIN);
  DL_GPIO_setPins(GPIO_MOTOR_CONTROL_PORT, GPIO_MOTOR_CONTROL_PIN_AIN2_PIN);
  DL_GPIO_setPins(GPIO_MOTOR_CONTROL_PORT, GPIO_MOTOR_CONTROL_PIN_BIN1_PIN);
  DL_GPIO_setPins(GPIO_MOTOR_CONTROL_PORT, GPIO_MOTOR_CONTROL_PIN_BIN2_PIN);
}
void motor_speed_set(uint16_t motor_left_speed, uint16_t motor_right_speed) {
  motor_left_speed = 100 - motor_left_speed;
  motor_right_speed = 100 - motor_right_speed;
  DL_TimerA_setCaptureCompareValue(PWM_MOTOR_INST, motor_left_speed * 10,
                                   GPIO_PWM_MOTOR_C0_IDX);
  DL_TimerA_setCaptureCompareValue(PWM_MOTOR_INST, motor_right_speed * 10,
                                   GPIO_PWM_MOTOR_C1_IDX);
}

int main(void) {
  SYSCFG_DL_init();
  SysTick_Init();
  MPU6050_Init();
  OLED_Init();

  while (1) {
    //-------------------------------------------------------------------------
    // oled display
    sprintf(mode_str, "MODE AT-%d", mode);
    OLED_ShowString(0, 0, (uint8_t *)mode_str, 8);
    if (sys_run == 1) {
      OLED_ShowString(64, 0, (uint8_t *)"GO RUNNING", 8);
    }
    if (sys_run == 0) {
      OLED_ShowString(64, 0, (uint8_t *)"STAND BY  ", 8);
    }
    sprintf((char *)oled_buffer, "ANGEL %-6.1f", yaw);
    OLED_ShowString(0, 6, oled_buffer, 16);
    //-------------------------------------------------------------------------

    //---------------------------------------------------------------------------
    // switch mode & control system starting
    if (DL_GPIO_readPins(GPIO_MODE_SELECT_PIN_SWITCH_PORT,
                         GPIO_MODE_SELECT_PIN_SWITCH_PIN) != 0) {
      if (mode < 4) {
        mode = mode + 1;
        delay_ms(200);
      } else {
        mode = 1;
        delay_ms(200);
      }
    }
    if (DL_GPIO_readPins(GPIO_MODE_SELECT_PIN_RUN_PORT,
                         GPIO_MODE_SELECT_PIN_RUN_PIN) == 0) {
      if (sys_run == 0) {
        sys_run = 1;
        delay_ms(200);
      } else {
        sys_run = 0;
        delay_ms(200);
      }
      angel_init = yaw;
    }
    //---------------------------------------------------------------------------

    if (sys_run == 0) {
      motor_stop();
    }
    if (mode == 1 && sys_run == 1) {
      while (1) {
        motor_run();
        motor_speed_set(line_speed, line_speed);
        sprintf((char *)oled_buffer, "ANGEL %-6.1f", yaw);
        OLED_ShowString(0, 6, oled_buffer, 16);
        delta = (int)trunc(yaw - angel_init);
        if (delta > 0) {
          motor_speed_set(line_speed - 5, line_speed);
          delay_ms(10);
        }
        if (delta < 0) {
          motor_speed_set(line_speed, line_speed - 5);
          delay_ms(10);
        }
        if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_1_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_2_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_3_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_4_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_5_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_6_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_7_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_8_PIN) == 0) {
          motor_stop();
          sys_run = 0;
          DL_GPIO_clearPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_0_PIN);
          DL_GPIO_setPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_LED_PIN);
          delay_ms(10);
          DL_GPIO_setPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_0_PIN);
          DL_GPIO_clearPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_LED_PIN);
          break;
        }
      }
    }
    if (mode == 2 && sys_run == 1) {
      while (situation == 1) {
        motor_run();
        motor_speed_set(line_speed, line_speed);
        sprintf((char *)oled_buffer, "ANGEL %-6.1f", yaw);
        OLED_ShowString(0, 6, oled_buffer, 16);
        delta = (int)trunc(yaw - angel_init);
        if (delta > 0) {
          motor_speed_set(line_speed - 5, line_speed);
          delay_ms(10);
        }
        if (delta < 0) {
          motor_speed_set(line_speed, line_speed - 5);
          delay_ms(10);
        }
        if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_1_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_2_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_3_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_4_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_5_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_6_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_7_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_8_PIN) == 0) {
          motor_stop();
          DL_GPIO_clearPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_0_PIN);
          DL_GPIO_setPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_LED_PIN);
          delay_ms(10);
          DL_GPIO_setPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_0_PIN);
          DL_GPIO_clearPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_LED_PIN);
          break;
        }
      }
      delay_ms(100);
      situation = 2;
      while (situation == 2) {
        motor_run();
        motor_speed_set(round_speed, round_speed);
        sprintf((char *)oled_buffer, "ANGEL %-6.1f", yaw);
        OLED_ShowString(0, 6, oled_buffer, 16);
        if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_3_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_2_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_1_PIN) == 0) {
          motor_speed_set(round_speed, round_speed - 10);
          delay_ms(20);
        }
        if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_6_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_7_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_8_PIN) == 0) {
          motor_speed_set(round_speed - 10, round_speed);

          delay_ms(20);
        }
        if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_1_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_2_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_3_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_4_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_5_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_6_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_7_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_8_PIN) != 0) {
          motor_stop();
          DL_GPIO_clearPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_0_PIN);
          delay_ms(10);
          DL_GPIO_setPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_0_PIN);
          angel_init = -177;
          break;
        }
      }
      delay_ms(100);
      situation = 3;
      while (situation == 3) {
        motor_run();
        motor_speed_set(line_speed, line_speed);
        sprintf((char *)oled_buffer, "ANGEL %-6.1f", yaw);
        OLED_ShowString(0, 6, oled_buffer, 16);
        delta = (int)trunc(yaw - angel_init);
        if (delta > 0) {
          motor_speed_set(line_speed - 5, line_speed);
          delay_ms(10);
        }
        if (delta < 0) {
          motor_speed_set(line_speed, line_speed - 5);
          delay_ms(10);
        }
        if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_1_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_2_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_3_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_4_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_5_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_6_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_7_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_8_PIN) == 0) {
          motor_stop();
          DL_GPIO_clearPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_0_PIN);
          DL_GPIO_setPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_LED_PIN);
          delay_ms(10);
          DL_GPIO_setPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_0_PIN);
          DL_GPIO_clearPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_LED_PIN);
          break;
        }
      }
      delay_ms(100);
      situation = 4;
      while (situation == 4) {
        motor_run();
        motor_speed_set(round_speed, round_speed);
        sprintf((char *)oled_buffer, "ANGEL %-6.1f", yaw);
        OLED_ShowString(0, 6, oled_buffer, 16);
        if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_3_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_2_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_1_PIN) == 0) {
          motor_speed_set(round_speed, round_speed - 10);
          delay_ms(20);
        }
        if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_6_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_7_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_8_PIN) == 0) {
          motor_speed_set(round_speed - 10, round_speed);

          delay_ms(20);
        }
        if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_1_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_2_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_3_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_4_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_5_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_6_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_7_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_8_PIN) != 0) {
          motor_stop();
          DL_GPIO_clearPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_0_PIN);
          delay_ms(10);
          DL_GPIO_setPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_0_PIN);
          motor_stop();
          situation = 1;
          sys_run = 0;
          break;
        }
      }
    }
    if (mode == 3 && sys_run == 1) {
      float angel_first, angel_second, angel_third;
      angel_second = 177;
      angel_third = -138;
      angel_first = angel_init - 45;
      motor_run();
      motor_speed_set(detect_speed, detect_speed);
      delay_ms(1200);
      while (situation == 1) { // 慢慢出去一点后找45°角
        motor_run();
        motor_speed_set(detect_speed, detect_speed);
        delta = (int)trunc(yaw - angel_first);
        if (delta > 0) {
          motor_speed_set(detect_speed - 5, detect_speed);
          delay_ms(10);
        }
        if (delta < 0) {
          motor_speed_set(detect_speed, detect_speed - 5);
          delay_ms(10);
        }
        if (delta == 0) {
          motor_stop();
          situation = 2;
          break;
        }
      }
      while (situation == 2) { // 沿着斜线走
        motor_run();
        motor_speed_set(round_speed, round_speed);
        sprintf((char *)oled_buffer, "ANGEL %-6.1f", yaw);
        OLED_ShowString(0, 6, oled_buffer, 16);
        delta = (int)trunc(yaw - angel_first);
        if (delta > 0) {
          motor_speed_set(round_speed - 5, round_speed);
          delay_ms(10);
        }
        if (delta < 0) {
          motor_speed_set(round_speed, round_speed - 5);
          delay_ms(10);
        }
        if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_1_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_2_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_3_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_4_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_5_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_6_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_7_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_8_PIN) == 0) {
          motor_stop();
          situation = 1;
          DL_GPIO_clearPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_0_PIN);
          DL_GPIO_setPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_LED_PIN);
          delay_ms(10);
          DL_GPIO_setPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_0_PIN);
          DL_GPIO_clearPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_LED_PIN);
          motor_run();
          DL_GPIO_clearPins(GPIO_MOTOR_CONTROL_PORT,
                            GPIO_MOTOR_CONTROL_PIN_AIN1_PIN);
          DL_GPIO_setPins(GPIO_MOTOR_CONTROL_PORT,
                          GPIO_MOTOR_CONTROL_PIN_AIN2_PIN);
          DL_GPIO_setPins(GPIO_MOTOR_CONTROL_PORT,
                          GPIO_MOTOR_CONTROL_PIN_BIN1_PIN);
          DL_GPIO_setPins(GPIO_MOTOR_CONTROL_PORT,
                          GPIO_MOTOR_CONTROL_PIN_BIN2_PIN);
          motor_speed_set(detect_speed, detect_speed);
          delay_ms(800);
          motor_stop();
          break;
        }
      }
      situation = 3;
      while (situation == 3) { // 巡线走
        motor_run();
        motor_speed_set(round_speed, round_speed);
        sprintf((char *)oled_buffer, "ANGEL %-6.1f", yaw);
        OLED_ShowString(0, 6, oled_buffer, 16);
        if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_3_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_2_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_1_PIN) == 0) {
          motor_speed_set(round_speed, round_speed - 15);
          delay_ms(20);
        }
        if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_6_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_7_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_8_PIN) == 0) {
          motor_speed_set(round_speed - 15, round_speed);
          delay_ms(20);
        }
        if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_1_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_2_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_3_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_4_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_5_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_6_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_7_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_8_PIN) != 0) {
          motor_stop();
          DL_GPIO_clearPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_0_PIN);
          delay_ms(10);
          DL_GPIO_setPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_0_PIN);
          break;
        }
      }
      situation = 4;
      while (situation == 4) { // 弯道出去后归零
        motor_run();
        motor_speed_set(detect_speed, detect_speed);
        delta = (int)trunc(yaw - angel_second);
        if (delta > 0) {
          motor_speed_set(detect_speed - 5, detect_speed);
          delay_ms(10);
        }
        if (delta < 0) {
          motor_speed_set(detect_speed, detect_speed - 5);
          delay_ms(10);
        }
        if (delta == 0) {
          motor_run();
          motor_speed_set(detect_speed, detect_speed);
          delay_ms(500);
          motor_stop();
          situation = 5;
          break;
        }
      }
      while (situation == 5) { // 第二个斜线，沿着走
        motor_run();
        motor_speed_set(round_speed, round_speed);
        sprintf((char *)oled_buffer, "ANGEL %-6.1f", yaw);
        OLED_ShowString(0, 6, oled_buffer, 16);
        delta = (int)trunc(yaw - angel_third);
        if (delta > 0) {
          motor_speed_set(round_speed - 5, round_speed);
          delay_ms(10);
        }
        if (delta < 0) {
          motor_speed_set(round_speed, round_speed - 5);
          delay_ms(10);
        }
        if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_1_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_2_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_3_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_4_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_5_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_6_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_7_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_8_PIN) == 0) {
          motor_stop();
          situation = 6;
          DL_GPIO_clearPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_0_PIN);
          DL_GPIO_setPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_LED_PIN);
          delay_ms(10);
          DL_GPIO_setPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_0_PIN);
          DL_GPIO_clearPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_LED_PIN);
          motor_run();
          DL_GPIO_setPins(GPIO_MOTOR_CONTROL_PORT,
                          GPIO_MOTOR_CONTROL_PIN_AIN1_PIN);
          DL_GPIO_setPins(GPIO_MOTOR_CONTROL_PORT,
                          GPIO_MOTOR_CONTROL_PIN_AIN2_PIN);
          DL_GPIO_clearPins(GPIO_MOTOR_CONTROL_PORT,
                            GPIO_MOTOR_CONTROL_PIN_BIN1_PIN);
          DL_GPIO_setPins(GPIO_MOTOR_CONTROL_PORT,
                          GPIO_MOTOR_CONTROL_PIN_BIN2_PIN);
          motor_speed_set(detect_speed, detect_speed);
          delay_ms(800);
          motor_stop();
          break;
        }
      }
      while (situation == 6) { // 巡线走
        motor_run();
        motor_speed_set(round_speed, round_speed);
        sprintf((char *)oled_buffer, "ANGEL %-6.1f", yaw);
        OLED_ShowString(0, 6, oled_buffer, 16);
        if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_3_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_2_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_1_PIN) == 0) {
          motor_speed_set(round_speed, round_speed - 15);
          delay_ms(20);
        }
        if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_6_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_7_PIN) == 0 ||
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_8_PIN) == 0) {
          motor_speed_set(round_speed - 15, round_speed);
          delay_ms(20);
        }
        if (DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_1_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_2_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_3_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_4_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_5_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_6_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_7_PIN) != 0 &&
            DL_GPIO_readPins(GPIO_SEARCH_LINE_PORT,
                             GPIO_SEARCH_LINE_PIN_8_PIN) != 0) {
          motor_stop();
          DL_GPIO_clearPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_0_PIN);
          delay_ms(10);
          DL_GPIO_setPins(GPIO_FUCK_TI_PORT, GPIO_FUCK_TI_PIN_0_PIN);
          situation = 1;
          break;
        }
      }
    }
    if (mode == 4 && sys_run == 1) {
    }
  }
}
